#include <opencv2/opencv.hpp>
#include "zhnmat.hpp"
#include "utils.cpp"
constexpr int CHECKERBOARD[2]{6,9};
constexpr double squareSize{28};
int main()
{
    using namespace std;
    using namespace cv;
    vector<vector<Point3f>> objpoints;
    vector<vector<Point2f>> imgpoints1, imgpoints2;
    vector<Point3f> objp;
    for(int i{0}; i<CHECKERBOARD[1]; i++)
        for(int j{0}; j<CHECKERBOARD[0]; j++)
            objp.push_back(Point3f(j*squareSize,i*squareSize,0));
    vector<String> images;
    glob("/home/pi/Desktop/myfile/images/stero/*.png", images);
    Mat origin, gray;
    vector<Point2f> corner_pts;
    bool success;
    TermCriteria criteria(TermCriteria::EPS | TermCriteria::MAX_ITER, 30, 0.001);
    for(int i{0}; i<images.size(); i++) {
        origin = imread(images[i]);
        cvtColor(origin(Rect(0, 0, 640, 480)), gray, COLOR_BGR2GRAY);
        success = findChessboardCorners(gray, Size(CHECKERBOARD[0], CHECKERBOARD[1]), corner_pts,
            CALIB_CB_ADAPTIVE_THRESH | CALIB_CB_FAST_CHECK | CALIB_CB_NORMALIZE_IMAGE);
        ASSERT_ERROR(success, "detect left chessboard failed.  " << images[i]);
        cornerSubPix(gray,corner_pts,Size(11,11), Size(-1,-1),criteria);
        imgpoints1.push_back(corner_pts);
        cvtColor(origin(Rect(640, 0, 640, 480)), gray, COLOR_BGR2GRAY);
        success = findChessboardCorners(gray, Size(CHECKERBOARD[0], CHECKERBOARD[1]), corner_pts,
            CALIB_CB_ADAPTIVE_THRESH | CALIB_CB_FAST_CHECK | CALIB_CB_NORMALIZE_IMAGE);
        ASSERT_ERROR(success, "detect right chessboard failed.  " << images[i]);
        cornerSubPix(gray,corner_pts,Size(11,11), Size(-1,-1),criteria);
        imgpoints2.push_back(corner_pts);
        objpoints.push_back(objp);
    }
    Mat cameraMatrix1 = (cv::Mat_<double>(3, 3)<<407.9841613769531, 0, 338.5738830721948, 0, 406.6146545410156, 248.5704179671375, 0, 0, 1);
    Mat cameraMatrix2 = (cv::Mat_<double>(3, 3)<<409.9005737304688, 0, 341.7043262459847, 0, 408.2314758300781, 237.9734030441668, 0, 0, 1);
    Mat distCoeffs1 = (cv::Mat_<double>(1, 5)<<-0.03474238292182809, 0.1016125734404889, -0.0001251312601070871, 0.002464460844023793, -0.08333277474283739);
    Mat distCoeffs2 = (cv::Mat_<double>(1, 5)<<-0.02750486103185542, 0.08729304265640324, -9.064484083648485e-05, 0.002019984835747687, -0.07174205621213117);
    Mat R, T, E, F;
    Size imageSize = Size(640, 480);
    double rms = stereoCalibrate(objpoints, imgpoints1, imgpoints2, cameraMatrix1, distCoeffs1, cameraMatrix2, distCoeffs2, imageSize, R, T, E, F, CALIB_FIX_INTRINSIC);
    Mat R1, R2, P1, P2, Q;
    stereoRectify(cameraMatrix1, distCoeffs1, cameraMatrix2, distCoeffs2, imageSize, R, T, R1, R2, P1, P2, Q, CALIB_ZERO_DISPARITY, 1);
    PRINT_NAME_VALUE(rms);
    PRINT_NAME_VALUE(R);
    PRINT_NAME_VALUE(T);
    PRINT_NAME_VALUE(E);
    PRINT_NAME_VALUE(F);
    PRINT_NAME_VALUE(R1);
    PRINT_NAME_VALUE(R2);
    PRINT_NAME_VALUE(P1);
    PRINT_NAME_VALUE(P2);
    PRINT_NAME_VALUE(Q);
    zhnmat::Mat::OutputFormat = 0;
    zhnmat::Mat zhnP1 = cvMat2zhnMat(P1)(zhnmat::Rect(0, 0, 3, 3));
    zhnmat::Mat zhnP2 = cvMat2zhnMat(P2)(zhnmat::Rect(0, 0, 3, 3));
    PRINT_NAME_VALUE(zhnP1);
    PRINT_NAME_VALUE(zhnP2);
    // Mat mapl1, mapl2, mapr1, mapr2;
    // initUndistortRectifyMap(cameraMatrix1, distCoeffs1, R1, P1, imageSize, CV_16SC2, mapl1, mapl2);
    // initUndistortRectifyMap(cameraMatrix2, distCoeffs2, R2, P2, imageSize, CV_16SC2, mapr1, mapr2);
    // Mat imgL, imgR;
    // for(int i{0}; i<images.size(); i++) {
    //     origin = imread(images[i]);
    //     remap(origin(Rect(0, 0, 640, 480)), imgL, mapl1, mapl2, INTER_LINEAR);
    //     remap(origin(Rect(640, 0, 640, 480)), imgR, mapr1, mapr2, INTER_LINEAR);
    //     hconcat(imgL, imgR, origin);
    //     imshow("Image", origin);
    //     waitKey(0);
    // }
    return 0;
}
